#include "usart.h"
#include "drv_SA100.h"
#include "tim.h"
#ifdef FREERTOS
#include "cmsis_os.h"
#endif

#define h_SA100 htim1
#define SA100_CH TIM_CHANNEL_4
#define SA100_ACTIVE_CH HAL_TIM_ACTIVE_CHANNEL_4

/**
 * @brief Initializes the SA100 device.
 *
 * This function initializes the SA100 device by starting the timer interrupt, creating a binary semaphore,
 * initializing a first-order Kalman filter, and setting initial values for the capture index and flag.
 *
 * @return Returns 1 if the initialization is successful.
 */
int SA100_Init(Device_SA100_t *SA100_dev)
{
#ifdef LOG_MODE
    printf("SA100 init\n");
#endif
    HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_4);
#ifdef FREERTOS
    SA100_dev->SA100_Duty.xSA100GetPosSemap = xSemaphoreCreateBinary();
#endif
    FirstOrder_KalmanFilter_Init(&(SA100_dev->SA100_KF), 0.001f, 0.133f);
    SA100_dev->SA100_Duty.CapIndex = First_Rrising;
    SA100_dev->SA100_Duty.CapFlag = 0;
    return 1;
}

#if 0
/**
 * @brief Callback function for TIM IC capture event.
 *
 * This function is called when a capture event occurs on TIM1 channel 4.
 * It reads the captured values and updates the capture index and flag accordingly.
 *
 * @param htim Pointer to the TIM_HandleTypeDef structure.
 */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM1)
    {
        if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4)
        {
            CaptureDutyCycle(&SCA_Arm_lwb.SA100, htim);
        }
    }
}
#endif

/**
 * @brief Capture the duty cycle of the signal
 *
 * This function captures the duty cycle of the signal by reading the captured values from the timer.
 *
 * @param SA100_dev Pointer to the SA100 device structure
 * @param htim Pointer to the timer handle
 */
void CaptureDutyCycle(Device_SA100_t *SA100_dev, TIM_HandleTypeDef *htim)
{
#ifdef FREERTOS
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
#endif
    switch (SA100_dev->SA100_Duty.CapIndex)
    {
    case First_Rrising:
        SA100_dev->SA100_Duty.CapFlag = 0;
        SA100_dev->SA100_Duty.CapVal[0] = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_4);
        // printf("Capture CapVal0 is :%d\n",SA100_dev->SA100_Duty.CapVal[0]);
        SA100_dev->SA100_Duty.CapIndex = Falling;
        __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_4, TIM_INPUTCHANNELPOLARITY_FALLING);
        break;

    case Falling:
        SA100_dev->SA100_Duty.CapVal[1] = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_4);
        // printf("Capture CapVal1 is :%d\n",SA100_dev->SA100_Duty.CapVal[1]);
        SA100_dev->SA100_Duty.CapIndex = Second_Rising;
        __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_4, TIM_INPUTCHANNELPOLARITY_RISING);
        break;

    case Second_Rising:
        SA100_dev->SA100_Duty.CapVal[2] = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_4);
        // printf("Capture CapVal2 is :%d\n",SA100_dev->SA100_Duty.CapVal[2]);
        HAL_TIM_IC_Stop_IT(htim, TIM_CHANNEL_4);
        SA100_dev->SA100_Duty.CapIndex = First_Rrising;
        SA100_dev->SA100_Duty.CapFlag = 1;
#ifdef FREERTOS
        xSemaphoreGiveFromISR(SA100_dev->SA100_Duty.xSA100GetPosSemap, &xHigherPriorityTaskWoken);
        if (xHigherPriorityTaskWoken == pdTRUE)
        {
            portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
        }
#endif
        break;

    default:
        Error_Handler();
        break;
    }
}

/**
 * @brief Get the position information of the SA100 module.
 *
 * This function determines the position of the SA100 module by calculating its duty cycle. The duty cycle is calculated by measuring the high time of the pulse and its period.
 * If the CapFlag flag is set, indicating that new position data is available, the function will update the duty cycle calculation and start a timer to obtain the next position update.
 *
 * @return The position of the SA100 module in degrees.
 */
__attribute__((weak)) float SA100_GetPos(Device_SA100_t *SA100_dev)
{
    /* Check if new position data is available */

    if (SA100_dev->SA100_Duty.CapFlag)
    {
        /* Clear the new data flag */
        SA100_dev->SA100_Duty.CapFlag = 0;

        /* Calculate the period */

        if (SA100_dev->SA100_Duty.CapVal[2] > SA100_dev->SA100_Duty.CapVal[0])
        {
            SA100_dev->SA100_Duty.Period = SA100_dev->SA100_Duty.CapVal[2] - SA100_dev->SA100_Duty.CapVal[0];
        }
        else
        {
            SA100_dev->SA100_Duty.Period = SA100_dev->SA100_Duty.CapVal[2] + 0xffff + 1 - SA100_dev->SA100_Duty.CapVal[0];
        }

        /* Calculate the high time of the pulse */
        if (SA100_dev->SA100_Duty.CapVal[1] >= SA100_dev->SA100_Duty.CapVal[0])
        {
            SA100_dev->SA100_Duty.HighTime = SA100_dev->SA100_Duty.CapVal[1] - SA100_dev->SA100_Duty.CapVal[0];
        }
        else
        {
            SA100_dev->SA100_Duty.HighTime = 0xFFFF + 1 - SA100_dev->SA100_Duty.CapVal[0] + SA100_dev->SA100_Duty.CapVal[1];
        }

        /* Calculate the duty cycle */

        SA100_dev->SA100_Duty.Duty = (float)SA100_dev->SA100_Duty.HighTime / (float)SA100_dev->SA100_Duty.Period;
        SA100_dev->raw_angle = (float)(SA100_dev->SA100_Duty.Duty * 360.0f);
        SA100_dev->filtered_angle = FirstOrder_KalmanFilter_Update(&SA100_dev->SA100_KF, SA100_dev->raw_angle);
        /* Start a timer to capture the next position update */
        // HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_4);
        /* Return the position information in degrees */

        // return SA100_dev->filtered_angle;
        return SA100_dev->raw_angle;
    }

    // return SA100_dev->filtered_angle;
    return SA100_dev->raw_angle;
}

/**
 * @brief Calibrates and filters the SA100 device.
 *
 * This function calculates the period, high time of the pulse, and duty cycle of the SA100 device.
 * It also applies a first-order Kalman filter to the raw angle.
 */
void SA100_Calibrate_n_Filter(Device_SA100_t *SA100_dev)
{
    /* Calculate the period */

    if (SA100_dev->SA100_Duty.CapVal[2] > SA100_dev->SA100_Duty.CapVal[0])
    {
        SA100_dev->SA100_Duty.Period = SA100_dev->SA100_Duty.CapVal[2] - SA100_dev->SA100_Duty.CapVal[0];
    }
    else
    {
        SA100_dev->SA100_Duty.Period = SA100_dev->SA100_Duty.CapVal[2] + 0xffff + 1 - SA100_dev->SA100_Duty.CapVal[0];
    }

    /* Calculate the high time of the pulse */
    if (SA100_dev->SA100_Duty.CapVal[1] >= SA100_dev->SA100_Duty.CapVal[0])
    {
        SA100_dev->SA100_Duty.HighTime = SA100_dev->SA100_Duty.CapVal[1] - SA100_dev->SA100_Duty.CapVal[0];
    }
    else
    {
        SA100_dev->SA100_Duty.HighTime = 0xFFFF + 1 - SA100_dev->SA100_Duty.CapVal[0] + SA100_dev->SA100_Duty.CapVal[1];
    }

    /* Calculate the duty cycle */

    SA100_dev->SA100_Duty.Duty = (float)SA100_dev->SA100_Duty.HighTime / (float)SA100_dev->SA100_Duty.Period;
    SA100_dev->raw_angle = (float)(SA100_dev->SA100_Duty.Duty * 360.0f);
    SA100_dev->filtered_angle = FirstOrder_KalmanFilter_Update(&SA100_dev->SA100_KF, SA100_dev->raw_angle);

    HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_4);
}
/* USER CODE END 0 */
